#ifndef VI_MAP_6DOF_POSE_GRAPH_GEN_H_
#define VI_MAP_6DOF_POSE_GRAPH_GEN_H_

#include <memory>
#include <unordered_map>
#include <vector>

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <imu-integrator/common.h>
#include <simulation/generic-path-generator.h>
#include <vi-map/edge.h>
#include <vi-map/landmark.h>
#include <vi-map/mission.h>
#include <vi-map/pose-graph.h>
#include <vi-map/vertex.h>
#include <vi-map/vi-mission.h>

namespace vi_map {

class ViwlsGraph;

class SixDofPoseGraphGenerator {
 public:
  SixDofPoseGraphGenerator();
  virtual ~SixDofPoseGraphGenerator() {}

  void constructCamera();
  void generatePathAndLandmarks();
  void fillPosegraph(
      int num_of_vertices, double vertex_position_sigma,
      double vertex_rotation_sigma, const Eigen::Vector3d& gyro_bias,
      const Eigen::Vector3d& accel_bias, int num_of_no_noise_vertices);
  void addInertialResidualBlocks(bool fix_gyro_bias, bool fix_accel_bias);
  void addVisualResidualBlocks(
      bool fix_intrinsics, bool fix_landmark_positions);
  void fixFirstVertices(unsigned int num_of_fixed_vertices, bool fix_velocity);
  void corruptLandmarkPositions(double sigma);
  void solve(int max_num_of_iters);
  void copyDataFromPosegraph();
  void copyDataToPosegraph();

  // Leave all of these as public as this is just for testing.
  PoseGraph posegraph_;
  pose_graph::VertexIdList vertex_ids_;
  std::unordered_map<LandmarkId, Landmark::Ptr> landmarks_;
  std::unordered_map<LandmarkId, unsigned int> landmark_observation_count_;
  std::unordered_map<MissionId, std::shared_ptr<VIMission>> missions_;
  AlignedUnorderedMap<pose_graph::VertexId, Eigen::Vector3d>
      true_vertex_positions_;
  AlignedUnorderedMap<pose_graph::VertexId, Eigen::Quaterniond>
      true_vertex_rotations_;
  AlignedUnorderedMap<LandmarkId, Eigen::Vector3d> true_landmark_positions_;
  AlignedUnorderedMap<LandmarkId, aslam::VisualFrame::DescriptorsT>
      landmark_descriptors_;

  aslam::NCamera::Ptr cameras_;

  // Camera parameters.
  double distortion_param_;
  double fu_, fv_;
  double cu_, cv_;
  double res_u_, res_v_;

  // Path and landmark settings.
  test_trajectory_gen::PathAndLandmarkSettings settings_;

  // Landmark base position and rotation.
  Eigen::Quaterniond G_q_B_;
  Eigen::Vector3d G_p_B_;

  // IMU to camera transformation.
  Eigen::Quaterniond C_q_I_;
  Eigen::Vector3d C_p_I_;

  // Data generated by 6DOF trajectory generator.
  Eigen::Matrix3Xd positions_;
  Eigen::Matrix4Xd rotations_;
  Eigen::Matrix3Xd velocities_;
  Eigen::Matrix<double, imu_integrator::kImuReadingSize, Eigen::Dynamic>
      imu_data_;
  Eigen::VectorXd imu_timestamps_seconds_;

  ceres::Problem problem_;
  ceres::Solver::Summary summary_;

  // Containers for 7-element pose objects.
  Eigen::Matrix<double, 7, 1> base_frame_;
  typedef std::unordered_map<pose_graph::VertexId, int> VertexIdRotationMap;
  VertexIdRotationMap vertex_id_to_pose_idx_;
  Eigen::Matrix<double, 7, Eigen::Dynamic> keyframe_poses_;

  // Ordering is [orientation position] -> [xyzw xyz].
  Eigen::Matrix<double, 7, 1> dummy_7d_0_;
  Eigen::Matrix<double, 7, 1> dummy_7d_1_;

  // JPL C_T_I used for error terms.
  pose::Transformation C_T_I_JPL_;

  ceres::LocalParameterization* pose_parameterization_;

  static constexpr int kNumCameras = 1;
  static constexpr int kVisualFrameIndex = 0;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  unsigned int id_counter_;
};

};  // namespace vi_map

#endif  // VI_MAP_6DOF_POSE_GRAPH_GEN_H_
